#1 
gnome-terminal -- bash -c "export PX4_SIM_HOST_ADDR=192.168.16.25;source /opt/ros/foxy/setup.bash;source /home/zhao/project2/soft_simulate/install/setup.bash;ros2 run uav_swarms getlocalpos_node; exec bash"
gnome-terminal -- bash -c "export PX4_SIM_HOST_ADDR=192.168.16.25;source /opt/ros/foxy/setup.bash;source /home/zhao/project2/soft_simulate/install/setup.bash; ros2 launch uav_swarms avoid_node_launch.py; exec bash"


#2, tongxin
gnome-terminal -- bash -c "export PX4_SIM_HOST_ADDR=192.168.16.25;source /opt/ros/foxy/setup.bash;source /home/zhao/project2/soft_simulate/install/setup.bash; ros2 launch uav_swarms communication_multirotor_node0_launch.py; exec bash"

gnome-terminal -- bash -c "export PX4_SIM_HOST_ADDR=192.168.16.25;source /opt/ros/foxy/setup.bash; source /home/zhao/project2/soft_simulate/install/setup.bash;ros2 launch uav_swarms communication_multirotor_node1_launch.py; exec bash"

gnome-terminal -- bash -c "export PX4_SIM_HOST_ADDR=192.168.16.25;source /opt/ros/foxy/setup.bash; source /home/zhao/project2/soft_simulate/install/setup.bash;ros2 launch uav_swarms communication_multirotor_node2_launch.py; exec bash"




source /home/zhao/project2/soft_simulate/install/setup.bash; ros2 topic pub /uav_swarms/drone_0_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
source /home/zhao/project2/soft_simulate/install/setup.bash; ros2 topic pub /uav_swarms/drone_1_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
source /home/zhao/project2/soft_simulate/install/setup.bash; ros2 topic pub /uav_swarms/drone_2_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"






gnome-terminal -- bash -c "export PX4_SIM_HOST_ADDR=192.168.16.98;cd  /home/zhao/下载/jn/PX4-Autopilot/Tools; ./sitl_multiple_run.sh 8; exec bash"

# 启动 MAVROS 节点
gnome-terminal -- bash -c "export PX4_SIM_HOST_ADDR=192.168.16.98;source /opt/ros/foxy/setup.bash; ros2 run mavros mavros_node --remap __ns:=/drone_0 --ros-args -p fcu_url:=udp://:14540@localhost:14580 -p tgt_system:=1 -p gcs_url:=udp://:14550@127.0.0.1:14550;exec bash"
gnome-terminal -- bash -c "export PX4_SIM_HOST_ADDR=192.168.16.98;source /opt/ros/foxy/setup.bash; ros2 run mavros mavros_node --remap __ns:=/drone_1 --ros-args -p fcu_url:=udp://:14541@localhost:14581 -p tgt_system:=2 -p gcs_url:=udp://:14551@127.0.0.1:14551;exec bash"
gnome-terminal -- bash -c "export PX4_SIM_HOST_ADDR=192.168.16.98;source /opt/ros/foxy/setup.bash; ros2 run mavros mavros_node --remap __ns:=/drone_2 --ros-args -p fcu_url:=udp://:14542@localhost:14582 -p tgt_system:=3 -p gcs_url:=udp://:14552@127.0.0.1:14552;exec bash"


float64 longtitude
float64 latitude
float64 altitude

 ros2 topic pub /uav_swarms/drone_0/gps_move yolo_detect_message/msg/Geolocation "{longtitude: 25.1220287 ,latitude:122.3456435 ,altitude: 715.2093139949905}"
 ros2 topic pub /uav_swarms/drone_1/gps_move yolo_detect_message/msg/Geolocation "{longtitude: 25.1220287, latitude: 122.3456435, altitude: 715.2093139949905}"

 ros2 topic pub /uav_swarms/drone_2/gps_move yolo_detect_message/msg/Geolocation "{longtitude: 25.1220287, latitude: 122.3456435, altitude: 715.2093139949905}"






 # 启动 PX4 SITL 模拟器
gnome-terminal -- bash -c "export PX4_SIM_HOST_ADDR=192.168.16.4;cd /home/nvidia/demo_ws/PX4-Autopilot/Tools; ./sitl_multiple_run.sh 3; exec bash"

# 启动 MAVROS 节点
gnome-terminal -- bash -c "export PX4_SIM_HOST_ADDR=192.168.16.4;source /opt/ros/foxy/setup.bash; ros2 run mavros mavros_node  --remap __ns:=/drone_0 --ros-args -p fcu_url:=udp://:14540@localhost:14580 -p tgt_system:=1 -p gcs_url:=udp://@192.168.16.191:14550; exec bash"
gnome-terminal -- bash -c "export PX4_SIM_HOST_ADDR=192.168.16.4;source /opt/ros/foxy/setup.bash; ros2 run mavros mavros_node --remap __ns:=/drone_1 --ros-args -p fcu_url:=udp://:14541@localhost:14581 -p tgt_system:=2 -p gcs_url:=udp://@192.168.16.191:14551;exec bash"
gnome-terminal -- bash -c "export PX4_SIM_HOST_ADDR=192.168.16.4;source /opt/ros/foxy/setup.bash; ros2 run mavros mavros_node --remap __ns:=/drone_2 --ros-args -p fcu_url:=udp://:14542@localhost:14582 -p tgt_system:=3  -p gcs_url:=udp://@192.168.16.191:14552; exec bash"


ros2 topic pub /uav_swarms/drone_0_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
ros2 topic pub /uav_swarms/drone_1_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
ros2 topic pub /uav_swarms/drone_2_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
ros2 topic pub /uav_swarms/drone_3_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
ros2 topic pub /uav_swarms/drone_4_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
ros2 topic pub /uav_swarms/drone_5_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
ros2 topic pub /uav_swarms/drone_6_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
ros2 topic pub /uav_swarms/drone_7_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
ros2 topic pub /uav_swarms/drone_8_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
ros2 topic pub /uav_swarms/drone_9_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
ros2 topic pub /uav_swarms/drone_10_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
ros2 topic pub /uav_swarms/drone_11_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
ros2 topic pub /uav_swarms/drone_12_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
ros2 topic pub /uav_swarms/drone_13_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
ros2 topic pub /uav_swarms/drone_14_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"
ros2 topic pub /uav_swarms/drone_15_communication_server_topic yolo_detect_message/msg/CallServerStatus "{status: true}"


source /opt/ros/foxy/setup.bash; ros2 run mavros mavros_node  --remap __ns:=/drone_0 --ros-args -p fcu_url:=udp://:7880@192.168.16.98:14550 -p tgt_system:=1 -p gcs_url:=udp://@192.168.16.191
